/*
 * This file is part of the Sx Framework Library.
 * 
 * Copyright (C) 2013 University of Colorado Denver
 * <min.choi@ucdenver.edu> <shane.transue@ucdenver.edu>
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy 
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 
 * copies of the Software, and to permit persons to whom the Software is 
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in 
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 
 * DEALINGS IN THE SOFTWARE.
 */
#include <sxTransformation.h>

Sx::Math::Transformation::Transformation() {
	this->position = Math::Vector3f::Zero();
	this->scale = Math::Matrix3f::Identity();
	this->rotation = Math::Quaternionf::Identity();
	this->transform = Math::Matrix4f::Identity();

	this->compile();
}

Sx::Math::Transformation::Transformation(const Transformation& transform) {
	this->position = transform.position;
	this->rotation = transform.rotation;
	this->scale = transform.scale;

	this->compile();
}

Sx::Math::Transformation::~Transformation() {}

/*
 * The compilation of the internal 4x4 matrix is performed by first converting
 * the internal quaternion to a 3x3 rotation matrix. The scale is then applied
 * to the rotation matrix. The transformation is then converted to a
 * heterogeneous 4x4 matrix. (Eigen may provide a more elegant way of doing this
 * rather than manually copying the values). The position values are then
 * applied to the final 4x4 matrix.
 *	Total operations: quat -> 3x3 rotation matrix
 *                    3x3 matrix multiplication
 *                    12 assignments
 */
void Sx::Math::Transformation::compile() {
	Eigen::Matrix3f rot = this->rotation.toRotationMatrix();
	Eigen::Matrix3f scaleRot = this->scale * rot;
	this->transform(0, 0) = scaleRot(0, 0);
	this->transform(0, 1) = scaleRot(1, 0);
	this->transform(0, 2) = scaleRot(2, 0);
	this->transform(1, 0) = scaleRot(0, 1);
	this->transform(1, 1) = scaleRot(1, 1);
	this->transform(1, 2) = scaleRot(2, 1);
	this->transform(2, 0) = scaleRot(0, 2);
	this->transform(2, 1) = scaleRot(1, 2);
	this->transform(2, 2) = scaleRot(2, 2);
	this->transform(0, 3) = this->position.x();
	this->transform(1, 3) = this->position.y();
	this->transform(2, 3) = this->position.z();
	this->transform(3, 0) = 0.0f;
	this->transform(3, 1) = 0.0f;
	this->transform(3, 2) = 0.0f;
	this->transform(3, 3) = 1.0f;
}

Eigen::Vector3f Sx::Math::Transformation::getPosition() const {
	return this->position;
}

Eigen::Vector3f Sx::Math::Transformation::getScale() const {
	return Eigen::Vector3f(this->scale(0, 0), this->scale(1, 1), this->scale(2, 2));
}

Eigen::Quaternionf Sx::Math::Transformation::getRotation() const {
	return this->rotation;
}

const Eigen::Matrix4f Sx::Math::Transformation::toMatrix4f() const {
	return Eigen::Matrix4f(this->transform);
}

void Sx::Math::Transformation::addPositionX(float x) {
	this->position.x() += x;
	this->transform(0, 3) = this->position.x();
}

void Sx::Math::Transformation::addPositionY(float y) {
	this->position.y() += y;
	this->transform(1, 3) = this->position.y();
}

void Sx::Math::Transformation::addPositionZ(float z) {
	this->position.z() += z;
	this->transform(2, 3) = this->position.z();
}

void Sx::Math::Transformation::addPosition(float x, float y, float z) {
	this->position.x() += x;
	this->position.y() += y;
	this->position.z() += z;
	this->transform(0, 3) = this->position.x();
	this->transform(1, 3) = this->position.y();
	this->transform(2, 3) = this->position.z();
}

void Sx::Math::Transformation::addPosition(const Eigen::Vector3f& displacement) {
	this->position.x() += displacement.x();
	this->position.y() += displacement.y();
	this->position.z() += displacement.z();
	this->transform(0, 3) = this->position.x();
	this->transform(1, 3) = this->position.y();
	this->transform(2, 3) = this->position.z();
}

void Sx::Math::Transformation::setPositionX(float x) {
	this->position.x() = x;
	this->transform(0, 3) = x;
}

void Sx::Math::Transformation::setPositionY(float y) {
	this->position.y() = y;
	this->transform(1, 3) = y;
}

void Sx::Math::Transformation::setPositionZ(float z) {
	this->position.z() = z;
	this->transform(2, 3) = z;
}

void Sx::Math::Transformation::setPosition(float x, float y, float z) {
	this->setPosition(Eigen::Vector3f(x, y, z));
}

void Sx::Math::Transformation::setPosition(const Eigen::Vector3f& position) {
	this->position = position;
	this->transform(0, 3) = this->position.x();
	this->transform(1, 3) = this->position.y();
	this->transform(2, 3) = this->position.z();
}

void Sx::Math::Transformation::addScaleX(float x) {
	this->scale(0, 0) += x;
	this->compile();
}

void Sx::Math::Transformation::addScaleY(float y) {
	this->scale(1, 1) += y;
	this->compile();
}

void Sx::Math::Transformation::addScaleZ(float z) {
	this->scale(2, 2) += z;
	this->compile();
}

void Sx::Math::Transformation::addScale(float x, float y, float z) {
	this->scale(0, 0) += x;
	this->scale(1, 1) += y;
	this->scale(2, 2) += z;
	this->compile();
}

void Sx::Math::Transformation::addScale(const Eigen::Vector3f& scale) {
	this->scale(0, 0) += scale.x();
	this->scale(1, 1) += scale.y();
	this->scale(2, 2) += scale.z();
	this->compile();
}

void Sx::Math::Transformation::setScaleX(float x) {
	this->scale(0, 0) = x;
	this->compile();
}

void Sx::Math::Transformation::setScaleY(float y) {
	this->scale(1, 1) = y;
	this->compile();
}

void Sx::Math::Transformation::setScaleZ(float z) {
	this->scale(2, 2) = z;
	this->compile();
}

void Sx::Math::Transformation::setScale(float x, float y, float z) {
	this->scale(0, 0) = x;
	this->scale(1, 1) = y;
	this->scale(2, 2) = z;
	this->compile();
}

void Sx::Math::Transformation::setScale(const Eigen::Vector3f& scale) {
	this->scale(0, 0) = scale.x();
	this->scale(1, 1) = scale.y();
	this->scale(2, 2) = scale.z();
	this->compile();
}

void Sx::Math::Transformation::setRotation(const Eigen::Quaternionf& rotation) {
	this->rotation = rotation;
	this->compile();
}

void Sx::Math::Transformation::addRotationX(float angle) {
	Eigen::AngleAxisf angleAxis(angle, Eigen::Vector3f::UnitX());
	this->rotation *= Eigen::Quaternionf(angleAxis);
	this->compile();
}

void Sx::Math::Transformation::addRotationY(float angle) {
	Eigen::AngleAxisf angleAxis(angle, Eigen::Vector3f::UnitY());
	this->rotation *= Eigen::Quaternionf(angleAxis);
	this->compile();
}

void Sx::Math::Transformation::addRotationZ(float angle) {
	Eigen::AngleAxisf angleAxis(angle, Eigen::Vector3f::UnitZ());
	this->rotation *= Eigen::Quaternionf(angleAxis);
	this->compile();
}

void Sx::Math::Transformation::addRotation(const Eigen::Quaternionf& rotation) {
	this->rotation *= rotation;
	this->compile();
}

void Sx::Math::Transformation::fromMatrixOnly(const Eigen::Matrix4f& transform) {
	this->transform = transform;
}

Sx::Math::Transformation Sx::Math::Transformation::Identity() {
	return Transformation();
}

Eigen::Quaternionf Sx::Math::Transformation::EulerRotation(float psi, float phi, float theta) {
	float halfTheta = theta * 0.5f;
	float halfPhi = phi * 0.5f;
	float halfPsi = psi * 0.5f;

	float cosHalfPhi = std::cos(halfPhi);
	float sinHalfPhi = std::sin(halfPhi);

	float cosHalfTheta = std::cos(halfTheta);
	float sinHalfTheta = std::sin(halfTheta);

	float cosHalfPsi = std::cos(halfPsi);
	float sinHalfPsi = std::sin(halfPsi);

	Eigen::Quaternionf quat;
	quat.w() = cosHalfPhi * cosHalfTheta * cosHalfPsi + sinHalfPhi * sinHalfTheta * sinHalfPsi;
	quat.y() = sinHalfPhi * cosHalfTheta * cosHalfPsi - cosHalfPhi * sinHalfTheta * sinHalfPsi;
	quat.z() = cosHalfPhi * sinHalfTheta * cosHalfPsi + sinHalfPhi * cosHalfTheta * sinHalfPsi;
	quat.x() = cosHalfPhi * cosHalfTheta * sinHalfPsi - sinHalfPhi * sinHalfTheta * cosHalfPsi;
	return quat;
}

Sx::Math::Transformation& Sx::Math::Transformation::operator = (const Transformation& t) {
	if ( this != &t ) {
		this->rotation = t.rotation;
		this->position = t.position;
		this->scale = t.scale;
		this->transform = t.transform;
	}
	return *this;
}
